/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 10:54:53
 * @LastEditor: junhuizhou
 * @LastEditTime: 2021-01-05 21:06:04
 * @Description: header
 * @FilePath: \fzhtq\src\attprop.c
 */

#include <stdio.h>
#include <math.h>
#include "../include/attprop.h"
#include "../include/caltransmat.h"
#include "../include/calmatrix.h"

void AttInit(double att[7], double angle[3], double anglerate[3])
{
    /*轨道系基础*/
    /*求本体系相对轨道系三轴角度姿态四元数*/
    Euler2Quat(angle, att);
    /*求本体系相对轨道系三轴角速度*/
    att[4] = anglerate[0];
    att[5] = anglerate[1];
    att[6] = anglerate[2];
}

void AttKineProp(double att[7], double dh)
{
    double dq[4];
    double tmpnum;
    double vecnorm;
    int i;

    /* dQdt = 0.5*M'[Q]*W */
    // double tmpqmat[4][4], omegavec[4];
    // /* M'[Q] */
    // tmpqmat[0][0] = att[0];
    // tmpqmat[0][1] = -att[1];
    // tmpqmat[0][2] = -att[2];
    // tmpqmat[0][3] = -att[3];
    // tmpqmat[1][0] = att[1];
    // tmpqmat[1][1] = att[0];
    // tmpqmat[1][2] = -att[3];
    // tmpqmat[1][3] = att[2];
    // tmpqmat[2][0] = att[2];
    // tmpqmat[2][1] = att[3];
    // tmpqmat[2][2] = att[0];
    // tmpqmat[2][3] = -att[1];
    // tmpqmat[3][0] = att[3];
    // tmpqmat[3][1] = -att[2];
    // tmpqmat[3][2] = att[1];
    // tmpqmat[3][3] = att[0];
    // /* W */
    // omegavec[0] = 0;
    // omegavec[1] = att[4];
    // omegavec[2] = att[5];
    // omegavec[3] = att[6];
    // /* M'[Q]*W */
    // MatMtp(dq, (double*)tmpqmat, 4, 4, omegavec, 4, 1);

    /* dQdt = 0.5*M[W]*Q */
    double tmpomegamat[4][4];
    tmpomegamat[0][0] = 0;
    tmpomegamat[0][1] = -att[4];
    tmpomegamat[0][2] = -att[5];
    tmpomegamat[0][3] = -att[6];
    tmpomegamat[1][0] = att[4];
    tmpomegamat[1][1] = 0;
    tmpomegamat[1][2] = att[6];
    tmpomegamat[1][3] = -att[5];
    tmpomegamat[2][0] = att[5];
    tmpomegamat[2][1] = att[6];
    tmpomegamat[2][2] = 0;
    tmpomegamat[2][3] = att[4];
    tmpomegamat[3][0] = att[6];
    tmpomegamat[3][1] = att[5];
    tmpomegamat[3][2] = -att[4];
    tmpomegamat[3][3] = 0;
    /* M[W]*Q */
    MatMtp(dq, (double*)tmpomegamat, 4, 4, att, 4, 1);

    /*q = q+dt*dq*/
    tmpnum = 0.5f * dh;
    MatMtp(dq, &tmpnum, 1, 1, dq, 4, 1);
    MatAdd(att, att, dq, 4, 1);
    /*归一化*/
    vecnorm = VectorNorm(att, 4);
    if(vecnorm>1e-6 || vecnorm<-1e-6)
    {
        tmpnum = 1.0 / vecnorm;
        MatMtp(att, &tmpnum, 1, 1, att, 4, 1);
    }

}

void AttDynaProp(double att[7], double JM[3][3], double JMI[3][3], double whlAxiAngMom[3], double Tq[3], double ha)
{
    /*****姿态运动学递推*****/
    AttKineProp(att, ha);

    /*****姿态动力学递推*****/
    double omegalast[3], omegatmp[3];
    double dwdtK[4][3];
    double tmp;
    double tmpvec1[3], tmpvec2[3];
    MatCpy(omegalast, &att[4], 3, 1);
    MatCpy(omegatmp, omegalast, 3, 1);

    /* K1 dwdtK[0][i]=f(omegalast) */
    /* I*w+Hm */
    MatMtp(tmpvec1, (double*)JM, 3, 3, omegalast, 3, 1);
    MatAdd(tmpvec1, tmpvec1, whlAxiAngMom, 3, 1);
    /* w x (I*w+Hm) */
    VectorCross(tmpvec2, omegalast, tmpvec1);
    /* Tq - w x (I*w+Hm) */
    tmp = -1;
    MatMtp(tmpvec2, &tmp, 1, 1, tmpvec2, 3, 1);
    MatAdd(tmpvec1, Tq, tmpvec2, 3, 1);
    /* I'(Tq - w x (I*w+Hm)) */
    MatMtp(tmpvec2, (double*)JMI, 3, 3, tmpvec1, 3, 1);
    MatCpy(dwdtK[0], tmpvec2, 3, 1);

    /* K2 f(omegatmp=omegalast+0.5*ha*K1) */
    /* omegatmp */
    tmp = 0.5*ha;
    MatMtp(tmpvec1, &tmp, 1, 1, dwdtK[0], 1, 3);
    MatAdd(omegatmp, omegalast, tmpvec1, 3, 1);
    /* I*w+Hm */
    MatMtp(tmpvec1, (double*)JM, 3, 3, omegatmp, 3, 1);
    MatAdd(tmpvec1, tmpvec1, whlAxiAngMom, 3, 1);
    /* w x (I*w+Hm) */
    VectorCross(tmpvec2, omegatmp, tmpvec1);
    /* Tq - w x (I*w+Hm) */
    tmp = -1;
    MatMtp(tmpvec2, &tmp, 1, 1, tmpvec2, 3, 1);
    MatAdd(tmpvec1, Tq, tmpvec2, 3, 1);
    /* I'(Tq - w x (I*w+Hm)) */
    MatMtp(tmpvec2, (double*)JMI, 3, 3, tmpvec1, 3, 1);
    MatCpy(dwdtK[1], tmpvec2, 3, 1);

    /* K3 f(omegatmp=omegalst+0.5*ha*K2) */
    /* omegatmp */
    tmp = 0.5*ha;
    MatMtp(tmpvec1, &tmp, 1, 1, dwdtK[1], 1, 3);
    MatAdd(omegatmp, omegalast, tmpvec1, 3, 1);
    /* I*w+Hm */
    MatMtp(tmpvec1, (double*)JM, 3, 3, omegatmp, 3, 1);
    MatAdd(tmpvec1, tmpvec1, whlAxiAngMom, 3, 1);
    /* w x (I*w+Hm) */
    VectorCross(tmpvec2, omegatmp, tmpvec1);
    /* Tq - w x (I*w+Hm) */
    tmp = -1;
    MatMtp(tmpvec2, &tmp, 1, 1, tmpvec2, 3, 1);
    MatAdd(tmpvec1, Tq, tmpvec2, 3, 1);
    /* I'(Tq - w x (I*w+Hm)) */
    MatMtp(tmpvec2, (double*)JMI, 3, 3, tmpvec1, 3, 1);
    MatCpy(dwdtK[2], tmpvec2, 3, 1);

    /* K4 f(omegatmp=omegalst+ha*K3) */
    /* omegatmp */
    tmp = ha;
    MatMtp(tmpvec1, &tmp, 1, 1, dwdtK[2], 1, 3);
    MatAdd(omegatmp, omegalast, tmpvec1, 3, 1);
    /* I*w+Hm */
    MatMtp(tmpvec1, (double*)JM, 3, 3, omegatmp, 3, 1);
    MatAdd(tmpvec1, tmpvec1, whlAxiAngMom, 3, 1);
    /* w x (I*w+Hm) */
    VectorCross(tmpvec2, omegatmp, tmpvec1);
    /* Tq - w x (I*w+Hm) */
    tmp = -1;
    MatMtp(tmpvec2, &tmp, 1, 1, tmpvec2, 3, 1);
    MatAdd(tmpvec1, Tq, tmpvec2, 3, 1);
    /* I'(Tq - w x (I*w+Hm)) */
    MatMtp(tmpvec2, (double*)JMI, 3, 3, tmpvec1, 3, 1);
    MatCpy(dwdtK[3], tmpvec2, 3, 1);

    /* (K1+2K2+2K3+K4)*ha/6 */
    MatAdd(tmpvec1, dwdtK[0], dwdtK[1], 1, 3);
    MatAdd(tmpvec1, tmpvec1, dwdtK[1], 1, 3);
    MatAdd(tmpvec1, tmpvec1, dwdtK[2], 1, 3);
    MatAdd(tmpvec1, tmpvec1, dwdtK[2], 1, 3);
    MatAdd(tmpvec1, tmpvec1, dwdtK[3], 1, 3);
    tmp = 1.0*ha/6.0;
    MatMtp(tmpvec2, &tmp, 1, 1, tmpvec1, 3, 1);

    /* omeganew = omegalast + (K1+2K2+2K3+K4)*ha/6 */
    MatAdd(&att[4], omegalast, tmpvec2, 3, 1);
}
